# -*- coding: utf-8 -*-
#!/usr/bin/env python3
import os
import rclpy
from rclpy.node import Node
import socket
import time
from customized_msgs.msg import DroneWholeState  # 导入自定义消息类型
from builtin_interfaces.msg import Time
import ast
# 配置目标 IP 和端口

DEFAULT_HOST = '0.0.0.0'  # 本机监听所有 IP 地址
DEFAULT_PORT = 12345       # 本地端口（接收数据）


# 配置 TCP 套接字参数
SOCKET_TIMEOUT = 5  # 超时时间（秒）

communication_frequency_max = 10

# 创建 TCP 套接字
def create_tcp_socket():
    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    s.settimeout(SOCKET_TIMEOUT)
    return s


# ROS 2 节点类
class TcpReceiverNode(Node):
    def __init__(self):
        super().__init__('tcp_receiver_node')

        self.declare_parameter('host', DEFAULT_HOST)
        self.declare_parameter('port', DEFAULT_PORT)

        self.host = self.get_parameter('host').get_parameter_value().string_value
        self.port = self.get_parameter('port').get_parameter_value().integer_value

        # 设置 TCP 套接字
        self.socket = create_tcp_socket()
        self.socket.bind((self.host, self.port))
        self.socket.listen(1)  # 只允许一个连接
        self.get_logger().info(f"Listening for connections on {self.host}:{self.port}")

        # 创建发布者
        self.publisher = self.create_publisher(DroneWholeState, '/winch_wireless_server/drone_whole_state', 20)

        # 启动接收数据线程
        self.timer = self.create_timer(1/communication_frequency_max, self.receive_data)
    # 解码接收到的数据
    def decode_message(self,message_str):
        # 这里假设数据是 CSV 格式：value1,value2,flag
        try:
            parts = message_str.split(',')
            # self.get_logger().info(f"part {len(parts)}")
            # self.get_logger().info(f'parts {parts}')
            if len(parts) == 21:
                odom_x = float(parts[0][1::])
                odom_y = float(parts[1])
                odom_z = float(parts[2])
                winch_length_control = float(parts[3])
                winch_length_control_flag = int(parts[4])
                ground_height = float(parts[5])
                battery_voltage = float(parts[6])
                arm_state = int(parts[7])
                tracking_error = float(parts[8])
                grab_process = int(parts[9])
                tag_seen_flag = int(parts[10])
                tag_z=float(parts[11])
                gcs_freq = float(parts[12])
                gps_satlites_count=int(parts[13])
                gps_status=int(parts[14])
                rssi1=int(parts[15])
                rssi2=int(parts[16])
                gps_yaw=int(parts[17])
                stamp_s=int(parts[18])
                stamp_n=int(parts[19])
                drone_status=(parts[20])
               
                drone_status = drone_status.encode('unicode_escape').decode('utf-8')


                # print(drone_status)
                # s1=ast.literal_eval(drone_status)
                # print(s1)
                # print(s1.decode('utf-8'))
                stamp = Time()
                stamp.sec = stamp_s
                stamp.nanosec = stamp_n
                
                return [[odom_x,odom_y,odom_z,\
                    winch_length_control,winch_length_control_flag,\
                    ground_height,battery_voltage,arm_state,tracking_error,\
                    grab_process,tag_seen_flag,tag_z,gcs_freq,gps_satlites_count,gps_status,rssi1,rssi2,gps_yaw,stamp,drone_status]]
                    
            else:
                self.get_logger().info('Receving multiple mesages')
                msg_len=message_str.split(';')
                result=[]
                for i in range(len(msg_len)):
                    parts = msg_len[i].split(',')
                    if len(parts) == 21:
                        odom_x = float(parts[0])
                        odom_y = float(parts[1])
                        odom_z = float(parts[2])
                        winch_length_control = float(parts[3])
                        winch_length_control_flag = int(parts[4])
                        ground_height = float(parts[5])
                        battery_voltage = float(parts[6])
                        arm_state = int(parts[7])
                        tracking_error = float(parts[8])
                        grab_process = int(parts[9])
                        tag_seen_flag = int(parts[10])
                        tag_z=float(parts[11])
                        gcs_freq = float(parts[12])
                        gps_satlites_count=int(parts[13])
                        gps_status=int(parts[14])
                        rssi1=int(parts[15])
                        rssi2=int(parts[16])
                        gps_yaw=int(parts[17])
                        
                        stamp_s=int(parts[18])
                        stamp_n=int(parts[19])
                        drone_status=(parts[20])
                        drone_status = drone_status.encode('unicode_escape').decode('utf-8')
                        stamp = Time()
                        stamp.sec = stamp_s
                        stamp.nanosec = stamp_n
                        result.append([odom_x,odom_y,odom_z,\
                            winch_length_control,winch_length_control_flag,\
                            ground_height,battery_voltage,arm_state,tracking_error,\
                            grab_process,tag_seen_flag,tag_z,gcs_freq,gps_satlites_count,gps_status,rssi1,rssi2,gps_yaw,stamp,drone_status])
                return result
        except Exception as e:
            self.get_logger().info(f'decode error :{e}')
            return None

    def receive_data(self):
        try:
            # 接受连接
            # self.get_logger().info("Waiting for a client to connect...")

            self.get_logger().info( f"Drone PID: {os.getpid()}: Waiting for a client to connect... ")
            conn, addr = self.socket.accept()
            conn.settimeout(1)
            self.get_logger().info(f"Drone PID: {os.getpid()}:  Connection established with {addr}")
            
            while True:
                try:
                    # 接收数据
                    data = conn.recv(1024)  # 假设每次接收的数据最大为 1024 字节
                    if data:
                        message_str = data.decode('utf-8')
                        self.get_logger().info(f"Received message: {message_str}")

                        # 解码数据
                        decoded = self.decode_message(message_str)
                        if decoded==None:
                            self.get_logger().warn("Failed to decode the message.deecoded ==none")
                            self.get_logger().info(f"Received data: {message_str}")
                            continue
                        if len(decoded)==1:
                            odom_x,odom_y,odom_z,\
                            winch_length_control,winch_length_control_flag,\
                            ground_height,battery_voltage,arm_state,tracking_error,\
                            grab_process,tag_seen_flag,tag_z,gcs_freq,gps_satlites_count,gps_status,rssi1,rssi2,gps_yaw,stamp,drone_status= decoded[0]

                            # 创建并发布消息
                            msg = DroneWholeState()
                            msg.odom_x = odom_x
                            msg.odom_y = odom_y
                            msg.odom_z = odom_z
                            msg.winch_length_control = winch_length_control
                            msg.winch_length_control_flag = winch_length_control_flag
                            msg.ground_height = ground_height
                            msg.battery_voltage = battery_voltage
                            msg.arm_state = arm_state
                            msg.tracking_error = tracking_error
                            msg.grab_process = grab_process
                            msg.tag_seen_flag = tag_seen_flag
                            msg.tag_z=tag_z
                            msg.gcs_freq = gcs_freq
                            msg.gps_satlites_count=gps_satlites_count
                            msg.gps_status=gps_status
                            msg.rssi1=rssi1
                            msg.rssi2=rssi2
                            msg.gps_yaw=gps_yaw
                            msg.stamp=stamp
                            msg.drone_status=drone_status
                           


                            self.publisher.publish(msg)
                            self.get_logger().info(f"Published: odom_x={odom_x}, odom_y={odom_y}, odom_z={odom_z}, "
                            f"winch_length_control={winch_length_control}, winch_length_control_flag={winch_length_control_flag}, "
                            f"ground_height={ground_height}, battery_voltage={battery_voltage}, arm_state={arm_state}, "
                            f"tracking_error={tracking_error}, grab_process={grab_process}, tag_seen_flag={tag_seen_flag}, "
                            f"gcs_freq={gcs_freq},msg.gps_satlites_count={gps_satlites_count},gps_status={gps_status},"
                            f"rssi1={rssi1},rssi2={rssi2}, gps_yaw={gps_yaw},stamp={stamp},drone_status={drone_status}")
                            
                            # self.get_logger().info(f"Published: value1={gcs_freq}, value2={tracking_error}, flag={tag_seen_flag}")
                        else:
                            for i in range(len(decoded)):
                                odom_x,odom_y,odom_z,\
                                winch_length_control,winch_length_control_flag,\
                                ground_height,battery_voltage,arm_state,tracking_error,\
                                grab_process,tag_seen_flag,tag_z,gcs_freq,gps_satlites_count,gps_status,rssi1,rssi2,gps_yaw,stamp,drone_status= decoded[i]

                                # 创建并发布消息
                                msg = DroneWholeState()
                                msg.odom_x = odom_x
                                msg.odom_y = odom_y
                                msg.odom_z = odom_z
                                msg.winch_length_control = winch_length_control
                                msg.winch_length_control_flag = winch_length_control_flag
                                msg.ground_height = ground_height
                                msg.battery_voltage = battery_voltage
                                msg.arm_state = arm_state
                                msg.tracking_error = tracking_error
                                msg.grab_process = grab_process
                                msg.tag_seen_flag = tag_seen_flag
                                msg.tag_z=tag_z
                                msg.gcs_freq = gcs_freq
                                msg.gps_satlites_count=gps_satlites_count
                                msg.gps_status=gps_status
                                msg.rssi1=rssi1
                                msg.rssi2=rssi2
                                msg.gps_yaw=gps_yaw
                                msg.stamp=stamp
                                msg.drone_status=drone_status
                               
                                self.publisher.publish(msg)
                                self.get_logger().info(f"{i}: Published : odom_x={odom_x}, odom_y={odom_y}, odom_z={odom_z}, "
                                    f"winch_length_control={winch_length_control}, winch_length_control_flag={winch_length_control_flag}, "
                                    f"ground_height={ground_height}, battery_voltage={battery_voltage}, arm_state={arm_state}, "
                                    f"tracking_error={tracking_error}, grab_process={grab_process}, tag_seen_flag={tag_seen_flag}, "
                                    f"gcs_freq={gcs_freq},msg.gps_satlites_count={gps_satlites_count},gps_status={gps_status},"
                                    f"rssi1={rssi1},rssi2={rssi2}, gps_yaw={gps_yaw},stamp={stamp},drone_status={drone_status}")
                                # self.get_logger().info(f"Published: value1={gcs_freq}, value2={tracking_error}, flag={tag_seen_flag}")
                    else:
                        self.get_logger().info("Connection closed by client. no data recv")
                        break  # 如果没有数据，退出循环，关闭连接
                except KeyboardInterrupt:
                    self.get_logger().info("KeyboardInterrupt")
                    break  # 捕获 Ctrl-C，退出循环，关闭连接
            self.get_logger().info("closing socket")
            conn.close()
        except socket.timeout:
            self.get_logger().warn("Socket timeout. No data received.")
        except Exception as e:
            self.get_logger().error(f"Error occurred: {e}")

def main(args=None):
    rclpy.init(args=args)
    node = TcpReceiverNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    
    main()

